
#include <opencv2\core\core.hpp>
#include <opencv2\imgproc\imgproc.hpp>
#include <opencv2\features2d\features2d.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2\legacy\legacy.hpp>

#include <vector>
#include <iostream>
#include <fstream>

using namespace cv;

Mat copnvert2flow(const Mat& velx, const Mat& vely)
{
    Mat flow(velx.size(), CV_32FC2);
    for(int y = 0 ; y < flow.rows; ++y)
        for(int x = 0 ; x < flow.cols; ++x)                        
            flow.at<Point2f>(y, x) = Point2f(velx.at<float>(y, x), vely.at<float>(y, x));            
    return flow;
}


void calcOpticalFlowLK( const Mat& prev, const Mat& curr, Size winSize,  cv::Mat& u,cv::Mat&v )
{
	
	int type = prev.type();
    Mat velx(prev.size(), CV_32F), vely(prev.size(), CV_32F); 
    CvMat cvvelx = velx;    CvMat cvvely = vely;
    CvMat cvprev = prev;    CvMat cvcurr = curr;


    cvCalcOpticalFlowLK( &cvprev, &cvcurr, winSize, &cvvelx, &cvvely );

	u = velx;
	v = vely;
}

void calcOpticalFlowLK( const Mat& prev, const Mat& curr, Size winSize,  cv::Mat& flow )
{
	cv::Mat u,v;
	calcOpticalFlowLK(prev,curr,winSize,u,v);
	flow = copnvert2flow(u, v);
}

double showFlowAndCalcError(const std:: string& name, const cv::Mat& gray, const cv::Mat& flow, 
                            const cv::Rect& where, const cv::Point& d, 
                            bool showImages = false, bool writeError = false)
{   
	using namespace cv;

    const int mult = 8;
	showImages=false;
    if (showImages)
    {
        Mat tmp, cflow;    
        resize(gray, tmp, gray.size() * mult, 0, 0, INTER_NEAREST);            
        cvtColor(tmp, cflow, CV_GRAY2BGR);        

        const float m2 = 0.3f;   
        const float minVel = 0.1f;

        for(int y = 0; y < flow.rows; ++y)
            for(int x = 0; x < flow.cols; ++x)
            {
                Point2f f = flow.at<Point2f>(y, x);                          

                if (f.x * f.x + f.y * f.y > minVel * minVel)
                {
                    Point p1 = Point(x, y) * mult;
                    Point p2 = Point(cvRound((x + f.x*m2) * mult), cvRound((y + f.y*m2) * mult));

                    line(cflow, p1, p2, CV_RGB(0, 255, 0));            
                    circle(cflow, Point(x, y) * mult, 2, CV_RGB(255, 0, 0));
                }            
            }

			static int N = 0 ;

        rectangle(cflow, (where.tl() + d) * mult, (where.br() + d - Point(1,1)) * mult, CV_RGB(0, 0, 255));    
		namedWindow(name, 1); imshow(name, cflow);imwrite("d:\\" + std::to_string((long long)N) + ".png",cflow);
		N++;
    }

    double angle = atan2((float)d.y, (float)d.x);
    double error = 0;

    bool all = true;
    Mat inner = flow(where);
    for(int y = 0; y < inner.rows; ++y)
        for(int x = 0; x < inner.cols; ++x)
        {
            const Point2f f = inner.at<Point2f>(y, x);

            if (f.x == 0 && f.y == 0)
                continue;

            all = false;

            double a = atan2(f.y, f.x);
            error += fabs(angle - a);            
        }
        double res = all ? std::numeric_limits<double>::max() : error / (inner.cols * inner.rows);

        if (writeError)
            std::cout << "Error " + name << " = " << res << std::endl;

		cv::waitKey();

        return res;
}



void convert_gray_to_rgb(const cv::Mat&gray, cv::Mat&rgb)
{
	for(int y = 0; y < gray.rows;y++)
		for(int x = 0; x < gray.cols;x++)
		{
			uchar pixel = gray.at<uchar>(cv::Point(x,y));
			rgb.at<cv::Vec3b>(cv::Point(x,y))[0] = pixel;		
			rgb.at<cv::Vec3b>(cv::Point(x,y))[1] = pixel;		
			rgb.at<cv::Vec3b>(cv::Point(x,y))[2] = pixel;
		}


}

void plot_lines(const cv::Mat &img,const std::vector<cv::Point2f>&orig,const std::vector<cv::Point2f>&dest,cv::Mat &imgOut)
{	
	cv::Mat imgShow (img.rows,img.cols,16); //cv::imread("C:\\Users\\William\\Compartilhado\\lk\\frame10.png");//(img.rows,img.cols,CV_32SC3 );
	
	convert_gray_to_rgb(img,imgShow);

	for(size_t i = 0 ; i < orig.size();i++)
	{
		if(abs(orig[i].x - dest[i].x )< 2 || abs(orig[i].y - dest[i].y )< 2 )
			continue;

		cv::line(imgShow,orig[i],dest[i],cv::Scalar(0,0,255,0));
		//imgShow.at<cv::Vec3i>((pts[i].y,pts[i].x))[1] = 0;
		//imgShow.at<cv::Vec3i>((pts[i].y,pts[i].x))[2] = 0;

		//imgShow.at<cv::Vec3i>(cv::Point(pts[i].x,pts[i].y))[0] = 255;
		//imgShow.at<cv::Vec3i>(cv::Point(pts[i].y,pts[i].x))[1] = 0;
		//imgShow.at<cv::Vec3i>(cv::Point(pts[i].y,pts[i].x))[2] = 0;
	}

	imgOut = imgShow;
}


void plot_points(const cv::Mat &img,const std::vector<cv::Point2f>&pts,cv::Mat &imgOut)
{
	cv::Mat imgShow (img.rows,img.cols,16); //cv::imread("C:\\Users\\William\\Compartilhado\\lk\\frame10.png");//(img.rows,img.cols,CV_32SC3 );
	
	convert_gray_to_rgb(img,imgShow);


	for(size_t i = 0 ; i < pts.size();i++)
	{

		cv::rectangle(imgShow,cv::Rect(pts[i].x-5,pts[i].y-5,11,11),cv::Scalar(0,0,255,0));

	}

	//cv::imshow("123",imgShow);
	//cv::waitKey();

	imgOut = imgShow;
	
}




int ymain( int argc, char** argv )
{

	try
	{
		const std::string strFrame1[3] = {"C:\\Users\\William\\Compartilhado\\lk\\frame10.png","d:\\mat0.bmp","C:\\Users\\William\\Documents\\Faculdade\\Implementacao2\\Urban3\\frame10.png"};
		const std::string strFrame2[3] = {"C:\\Users\\William\\Compartilhado\\lk\\frame11.png","d:\\mat1.bmp","C:\\Users\\William\\Documents\\Faculdade\\Implementacao2\\Urban3\\frame11.png"};


		cv::Mat frame1 = cv::imread(strFrame1[2],0);
		cv::Mat frame2 = cv::imread(strFrame2[2],0);
		cv::Mat flow;//velX(frame1.size(),CV_32FC1),velY(frame2.size(),CV_32FC1);
		cv::Mat seq[2];

		std::vector<cv::Point2f>points[2];	
		cv::TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
		cv::Size winSize(15,15);

		cv::goodFeaturesToTrack(frame1, points[0], 500, 0.01, 10);

		
		
		cv::cornerSubPix(frame1, points[0], winSize, cv::Size(-1,-1), termcrit);
		
		
		plot_points(frame1,points[0],seq[0]);


		cv::imshow("opencv",seq[0]);
		cv::waitKey();
	}
	catch(std::exception &ex)
	{
		std::cout << ex.what();

	}

	cv::waitKey();
	return 1;
}
